#!/usr/bin/env python
import roslib; roslib.load_manifest('robcar_ros')
import rospy

from sensor_msgs.msg import LaserScan
from robcar_ros.msg import move1D, prob

world = eval(rospy.get_param('/map'))

m_p = rospy.get_param('/m_p')
z_p = rospy.get_param('/z_p')
pose_est = []
move = 0
scan = 0
ranges = []
pub = []


def init_vars():
	global pub
	global world
	global pose_est
	for i in range(len(world)):
		pose_est.append(1.0/len(world))
		
	pub = rospy.Publisher('prob_data', prob)

def normalize(l):
	s = sum(l)
	
	for i in range(len(l)):
		l[i] = l[i]/s
	
	return l
		
def estimate(est, move):
	new_est = []
	if move == 0:
		return est
	if move == 1:
		new_est = [0.0]
		for i in range(len(est)-1):
			new_est.append(est[i])
	if move == -1:
		new_est = []
		for i in range(len(est)-1):
			new_est.append(est[i+1])
		new_est.append(0.0)
		
	return new_est

def	publish_prob(pub,est):	
	
	prob_m = prob()
	
	prob_m.prob = est
	print "p=",prob_m
	print "p.p=",prob_m.prob
	pub.publish(prob_m)
		
def callback2(data):
	global ranges
	global scan
	
	ranges = data.ranges
	
	avg = sum(ranges)/float(len(ranges))
	
	scan = 0
	
	if avg <= 1.5:
		scan = 1
		
		
def callback(data):
	global move
	global pose_est
	global world
	global z_p
	global m_p	
	global pub
	
	move = data.move
	
	print "my move is %s" % (move)
	pose_est = estimate(pose_est,move)
	
	print "my estimation is %s" % pose_est
	
	print "my read is = %s" % scan	
	for i in range(len(world)):
		if (world[i]==scan):
			pose_est[i] = (pose_est[i]*z_p)
		else:
			pose_est[i] = (pose_est[i]*(1-z_p))
	
	pose_est = normalize(pose_est)
	
	print "My estimation after read is %s" % (pose_est)
	publish_prob(pub,pose_est)

def listener():
	rospy.init_node('pose_estimator', anonymous=True)
	rospy.Subscriber("move", move1D, callback)
	rospy.Subscriber("base_scan", LaserScan,callback2)
	
	rospy.spin()


if __name__ == '__main__':
    try:
    	init_vars()
    	listener()
        
    except rospy.ROSInterruptException:
    	pass
